 /*
 * trajectory.cpp
 *
 *  Created on: 2021年5月3日
 *      Author: zhiping fu
 */

#include "trajectory.hpp"


void TRAJECTORY::trajectory_Init()
{
	volcity_average = 0.1f;
	dis_sum = 0.0f;

	//初始化航点信息和轨迹信息
	waypoints.waypointsRecvFinishedFlag = 0;
	waypoints.numOfWaypoints = 0;
//
//	for(uint8_t i = 1; i < waypoints.numOfWaypoints; i++)
//	{
//		waypoints.waypoints[i].Pos_NED[0] = (float)i;
//		waypoints.waypoints[i].Pos_NED[1] = (float)i;
//	}

	trajectory_message.trajectory_Finished_Flag = 0;

}


void TRAJECTORY::trajectory_Run()
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	//临时添加，第一个点就是飞机此时的参考值
	xQueuePeek(queueInloopControl, &inloop_control, 0);
	xQueuePeek(queueHeight, &height, 0);
	waypoints.waypoints[0].Pos_NED[0] = inloop_control.setPosition[0];
	waypoints.waypoints[0].Pos_NED[1] = inloop_control.setPosition[1];
	waypoints.waypoints[0].Pos_NED[2] = inloop_control.setPosition[2];

	//获取航点数量以及航点信息
	numOfWaypoints = waypoints.numOfWaypoints;		//获取航点数量
	n_seg = numOfWaypoints - 1;                     //n_seg段轨迹
	//计算轨迹的总距离
	dis_sum = 0.0f;
	for(uint8_t i = 1; i <= n_seg; i++)
	{
		dis_nth[i-1] = sqrt((waypoints.waypoints[i].Pos_NED[0]-waypoints.waypoints[i-1].Pos_NED[0])*(waypoints.waypoints[i].Pos_NED[0]-waypoints.waypoints[i-1].Pos_NED[0])+(waypoints.waypoints[i].Pos_NED[1]-waypoints.waypoints[i-1].Pos_NED[1])*(waypoints.waypoints[i].Pos_NED[1]-waypoints.waypoints[i-1].Pos_NED[1])+(waypoints.waypoints[i].Pos_NED[2]-waypoints.waypoints[i-1].Pos_NED[2])*(waypoints.waypoints[i].Pos_NED[2]-waypoints.waypoints[i-1].Pos_NED[2]));
		dis_sum += dis_nth[i-1];
	}
	//飞机总的飞行时间T_sum
	T_sum = dis_sum/volcity_average;
	float T_sum_n_1_seg = 0.0f;
	//dis_nth每段轨迹的长度，for循环内只计算前n_seg-1段轨迹，最后一段轨迹时间单独计算，每段轨迹时间按照该段轨迹占轨迹总长比例分配时间
	for(uint8_t i = 0; i < n_seg-1; i++)
	{
		T_nth_seg[i] = dis_nth[i]/dis_sum * T_sum;
		T_sum_n_1_seg += T_nth_seg[i];
	}
	//最后一段轨迹时间T_nth_seg[n_seg - 1]
	T_nth_seg[n_seg - 1] = T_sum - T_sum_n_1_seg;
	//如果只有两个航路点，即只有一段轨迹，则总时间加倍
	if(n_seg == 1)
	{
		T_sum += T_nth_seg[0];
		T_nth_seg[0] = 2.0f*T_nth_seg[0];
	}
	//否则令第一段和最后一段轨迹的时间加倍，因为起点和终点的速度为0，时间加倍有利于无人机减速
	else if(n_seg > 1)
	{
		T_sum += (T_nth_seg[0]+T_nth_seg[n_seg - 1]);
		T_nth_seg[0] = 2.0f*T_nth_seg[0];
		T_nth_seg[n_seg - 1] = 2.0f*T_nth_seg[n_seg - 1];
	}

	//mininumSnap 闭式求解
	Q = MatrixXf::Zero(n_seg*(n_order+1), n_seg*(n_order+1));
	M = MatrixXf::Zero(8*n_seg, n_seg*(n_order+1));
	Ct = MatrixXf::Zero(8*n_seg, 4*n_seg+4);
	R = MatrixXf::Zero(4*n_seg+4, 4*n_seg+4);
	dF = VectorXf::Zero(n_seg+7);
	dP = VectorXf::Zero(3*(n_seg-1));
	dF_dP = VectorXf::Zero(4*n_seg+4);
	P = VectorXf::Zero(n_seg*(n_order+1));


	//计算Q阵
	getQ();

	//计算M阵
	getM();

	//计算Ct阵
	getCt();

	//计算R阵
	R = Ct.transpose() * (M.inverse()).transpose() * Q * M.inverse() * Ct;

	mininumSnapCloseSolver(n_state_x);
	for(uint8_t i = 0; i < (n_order+1)*n_seg; i++)
	{
		trajectory_message.poly_coef_x[i] = P(i);
	}
	mininumSnapCloseSolver(n_state_y);
	for(uint8_t i = 0; i < (n_order+1)*n_seg; i++)
	{
		trajectory_message.poly_coef_y[i] = P(i);
	}
	mininumSnapCloseSolver(n_state_z);
	for(uint8_t i = 0; i < (n_order+1)*n_seg; i++)
	{
		trajectory_message.poly_coef_z[i] = P(i);
	}

	//求解完成
	trajectory_message.n_seg = n_seg;
	for(uint8_t i = 0; i < n_seg; i++)
	{
		trajectory_message.ts[i] = T_nth_seg[i];
	}
	trajectory_message.trajectory_Finished_Flag = 1;


	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;


}


void TRAJECTORY::mininumSnapCloseSolver(eN_State n_state)
{
	switch(n_state)
	{
	case n_state_x:
		//计算dF
		dF(0) = waypoints.waypoints[0].Pos_NED[0];
		dF(1) = 0.0f;
		dF(2) = 0.0f;
		dF(3) = 0.0f;
		for(uint8_t i = 1; i <= n_seg-1; i++)
		{
			dF(i+3) = waypoints.waypoints[i].Pos_NED[0];
		}
		dF(n_seg+3) = waypoints.waypoints[numOfWaypoints-1].Pos_NED[0];
		dF(n_seg+4) = 0.0f;
		dF(n_seg+5) = 0.0f;
		dF(n_seg+6) = 0.0f;
		break;
	case n_state_y:
		//计算dF
		dF(0) = waypoints.waypoints[0].Pos_NED[1];
		dF(1) = 0.0f;
		dF(2) = 0.0f;
		dF(3) = 0.0f;
		for(uint8_t i = 1; i <= n_seg-1; i++)
		{
			dF(i+3) = waypoints.waypoints[i].Pos_NED[1];
		}
		dF(n_seg+3) = waypoints.waypoints[numOfWaypoints-1].Pos_NED[1];
		dF(n_seg+4) = 0.0f;
		dF(n_seg+5) = 0.0f;
		dF(n_seg+6) = 0.0f;
		break;
	case n_state_z:
		//计算dF
		dF(0) = waypoints.waypoints[0].Pos_NED[2];
		dF(1) = 0.0f;
		dF(2) = 0.0f;
		dF(3) = 0.0f;
		for(uint8_t i = 1; i <= n_seg-1; i++)
		{
			dF(i+3) = waypoints.waypoints[i].Pos_NED[2];
		}
		dF(n_seg+3) = waypoints.waypoints[numOfWaypoints-1].Pos_NED[2];
		dF(n_seg+4) = 0.0f;
		dF(n_seg+5) = 0.0f;
		dF(n_seg+6) = 0.0f;
		break;
	case n_state_psi:

		break;

	}


	//计算dP
	dP = -R.block(n_seg+7, n_seg+7, 3*(n_seg-1), 3*(n_seg-1)).inverse() * R.block(0, n_seg+7, n_seg+7, 3*(n_seg-1)).transpose() * dF;

	//计算P
	dF_dP.block(0, 0, n_seg+7, 1) = dF;
	dF_dP.block(n_seg+7, 0, 3*(n_seg-1), 1) = dP;

	P = M.inverse() * Ct * dF_dP;
}

void TRAJECTORY::getQ()
{
	for(uint8_t i = 0; i < n_seg; i++)
	{
		Q_nth.Zero();
		for(uint8_t r = 4; r <= n_order; r++)
		{
			for(uint8_t c = 4; c <= n_order; c++)
			{
				Q_nth(r,c) = (factorial(r)*factorial(c)*pow(T_nth_seg[i],r+c-7))/((r+c-7)*factorial(r-4)*factorial(c-4));
			}
		}
		Q.block((n_order+1)*i, (n_order+1)*i, (n_order+1), (n_order+1)) = Q_nth;
	}
}

void TRAJECTORY::getM()
{
	for(uint8_t i = 0; i < n_seg; i++)
	{
		M_nth.Zero();
		for(uint8_t r = 0; r <= 3; r++)
		{
			M_nth(r,r) = factorial(r);
		}

		for(uint8_t r = 4; r < 8; r++)
		{
			for(uint8_t c = r - 4; c <= n_order; c++)
			{
				M_nth(r,c) = (factorial(c)*pow(T_nth_seg[i],c-(r-4)))/factorial(c-(r-4));
			}
		}
		M.block(8*i, (n_order+1)*i, 8, (n_order+1)) = M_nth;
	}
}

void TRAJECTORY::getCt()
{
	Ct(0,0) = 1.0f;
	Ct(1,1) = 1.0f;
	Ct(2,2) = 1.0f;
	Ct(3,3) = 1.0f;
	uint16_t index = 4;
	for(uint8_t i = 0; i < n_seg-1; i++)
	{
		Ct(8*i+4,index) = 1.0f;
		Ct(8*i+8,index) = 1.0f;
		index++;
	}
	Ct(8*(n_seg-1)+4, index) = 1.0f;
	index++;
	Ct(8*(n_seg-1)+5, index) = 1.0f;
	index++;
	Ct(8*(n_seg-1)+6, index) = 1.0f;
	index++;
	Ct(8*(n_seg-1)+7, index) = 1.0f;
	index++;
	for(uint8_t i = 0; i < n_seg-1; i++)
	{
		Ct(8*i+5,index) = 1.0f;
		Ct(8*i+9,index) = 1.0f;
		index++;
		Ct(8*i+6,index) = 1.0f;
		Ct(8*i+10,index) = 1.0f;
		index++;
		Ct(8*i+7,index) = 1.0f;
		Ct(8*i+11,index) = 1.0f;
		index++;
	}

}


TRAJECTORY trajectory((char*) "Trajectory");

extern "C" void trajectory_main(void *argument)
{
	trajectory.trajectory_Init();
	osDelay(500);
	for(;;)
	{
		vTaskSuspend(trajectoryTaskHandle);
//		osDelay(5);
		trajectory.trajectory_Run();

	}
}




